% clear
clc
%% park transform
%park matrix cos
syms a
assume(a*3/pi,'integer')
Tcosf1t_a = zeros(3,3);
for i=2:3
    Tcosf1t_a(i-1,i)=0.5;
    Tcosf1t_a(i,i-1)=0.5;
end
Tcosf1t_a = sym(Tcosf1t_a);
Tcosf1t_b = Tcosf1t_a;
for i=2:3
    Tcosf1t_b(i-1,i) = Tcosf1t_a(i-1,i)*exp(1i*a);
    Tcosf1t_b(i,i-1) = Tcosf1t_a(i,i-1)*exp(-1i*a);
end
Tcosf1t_c = Tcosf1t_a;
for i=2:3
    Tcosf1t_c(i-1,i) = Tcosf1t_a(i-1,i)*exp(-1i*a);
    Tcosf1t_c(i,i-1) = Tcosf1t_a(i,i-1)*exp(1i*a);
end
%park matrix sin
Tsinf1t_a = zeros(3,3);
for i=2:3
    Tsinf1t_a(i-1,i)=1i*0.5;
    Tsinf1t_a(i,i-1)=(-1i*0.5);
end
Tsinf1t_a = sym(Tsinf1t_a);
Tsinf1t_b = Tsinf1t_a;
for i=2:3
    Tsinf1t_b(i-1,i) = Tsinf1t_a(i-1,i)*exp(1i*a);
    Tsinf1t_b(i,i-1) = Tsinf1t_a(i,i-1)*exp(-1i*a);
end
Tsinf1t_c = Tsinf1t_a;
for i=2:3
    Tsinf1t_c(i-1,i) = Tsinf1t_a(i-1,i)*exp(-1i*a);
    Tsinf1t_c(i,i-1) = Tsinf1t_a(i,i-1)*exp(1i*a);
end

%% forward matrix G
%transfor function of output filter, seen from converter side voltage to current in outer filter;
%  ^^^^(current)^^^....$$$(voltage)$$$$
%                   ..
%                   ^^
%                   ==     (^resistance =capacitor $inductance)
syms L1_s0 %TL(s-0*jw1)
syms L1_s1 %TL(s-1jw1)
syms L1_sn1 %TL(s+1jw1)
assume([L1_sn1,L1_s0,L1_s1],'real')
L1B = [L1_sn1,L1_s0,L1_s1];
TL1 = diag(L1B);
G = -inv(diag(L1B));
vpa(G,4);
%% input filter for passive filter
syms Kf_s0 %TL(s-0*jw1)
syms Kf_s1 %TL(s-1jw1)
syms Kf_sn1 %TL(s+1jw1)
assume([Kf_sn1,Kf_s0,Kf_s1],'real')
KfB = [Kf_sn1,Kf_s0,Kf_s1];
Kf = (diag(KfB));
vpa(Kf,4);
%% back matrix H
% H is current feedback path without effect of outer loop
vec_atob = diag([exp(1i*a),1,exp(-1i*a)]);
vec_atoc = diag([exp(-1i*a),1,exp(1i*a)]);

park_d = 2/3*(Tcosf1t_a+Tcosf1t_b*vec_atob+Tcosf1t_c*vec_atoc);
park_q = -2/3*(Tsinf1t_a+Tsinf1t_b*vec_atob+Tsinf1t_c*vec_atoc);

%transfor function of output filter, L = s*L;
syms Hi_s0 %TL(s-0*jw1)
syms Hi_s1 %TL(s-1jw1)
syms Hi_sn1 %TL(s+1jw1)
assume([Hi_sn1,Hi_s0,Hi_s1],'real')
HiB = [Hi_sn1,Hi_s0,Hi_s1];
THi = diag(HiB);
syms Kdq
assume(Kdq,'real')
Tkdq = diag(Kdq*ones(1,3));
syms Vdc0
assume(Vdc0,'real')
syms Km_sn1 Km_s0 Km_s1
assume([Km_sn1,Km_s0,Km_s1],'real')
KmB = [Km_sn1,Km_s0,Km_s1];
Km = diag(KmB);
TVdc0 = diag(Vdc0*ones(1,3));

Hi = (Tcosf1t_a*(-THi*park_d-Tkdq*park_q)-Tsinf1t_a*(-THi*park_q+Tkdq*park_d));
H = Km*TVdc0*Hi;
H = simplify(subs(H,a,2*pi/3));
% intermediate variable for display
H_disp = sym('H_disp',[3 3]).*nonzero_mask(Hi);

%% S for modelling PLL part
%transfor function of pll, theta = Tpll*vqs;
syms Tpll_s0 %Tpll(s-0*jw1)
syms Tpll_s1 %Tpll(s-1jw1)
syms Tpll_sn1 %Tpll(s+1jw1)
assume([Tpll_sn1,Tpll_s0,Tpll_s1],'real')

syms I1d I1q M1d M1q
assume([I1d,I1q,M1d,M1q],'real')
TI1d = diag(I1d*ones(1,3));
TI1q = diag(I1q*ones(1,3));
TM1d = diag(M1d*ones(1,3));
TM1q = diag(M1q*ones(1,3));


TpllB = [Tpll_sn1,Tpll_s0,Tpll_s1];
TTpll = diag(TpllB);

Spll = (Tcosf1t_a*(TI1q*(-THi)-TI1d*(-Tkdq))-Tsinf1t_a*(-TI1d*(-THi)+TI1q*Tkdq))*TTpll*park_q...
    +(-TM1q*Tcosf1t_a-TM1d*Tsinf1t_a)*TTpll*park_q;
S = Km*TVdc0*Spll;
S = simplify(subs(S,a,2*pi/3));
% intermediate variable for display
S_disp = sym('S_disp',[3 3]).*nonzero_mask(S);

%% F Cv Ci for modelling PLL part
syms I1a U1a M1a
TI1a = toeplitz([0,I1a,0],[0,conj(I1a),0])/2;
TI1b = toeplitz([0,I1a*exp(-1i*a),0],[0,conj(I1a)*exp(1i*a),0])/2;
TI1c = toeplitz([0,I1a*exp(1i*a),0],[0,conj(I1a)*exp(-1i*a),0])/2;
TU1a = toeplitz([0,U1a,0],[0,conj(U1a),0])/2;
TU1b = toeplitz([0,U1a*exp(-1i*a),0],[0,conj(U1a)*exp(1i*a),0])/2;
TU1c = toeplitz([0,U1a*exp(1i*a),0],[0,conj(U1a)*exp(-1i*a),0])/2;
TM1a = toeplitz([0,M1a,0],[0,conj(M1a),0])/2;

Cv_num = [TI1a,TI1b,TI1c]*[Kf*eye(3);Kf*vec_atob;Kf*vec_atoc];

syms Iload
assume(Iload,'real')
TIload = diag(Iload*ones(1,3));
syms Cdc_sn1 Cdc_s0 Cdc_s1
syms Zdcn_sn1 Zdcn_s0 Zdcn_s1
assume([Zdcn_sn1 Zdcn_s0 Zdcn_s1],'real')
assume([Cdc_sn1 Cdc_s0 Cdc_s1],'real')
TZdcn = diag([Zdcn_sn1 Zdcn_s0 Zdcn_s1]);
TCdc = diag([Cdc_sn1 Cdc_s0 Cdc_s1]);
C_den = TIload-2*TVdc0/(TZdcn)-TVdc0*TCdc;

Cv = Cv_num/C_den;
Cv = simplify(subs(Cv,a,2*pi/3));
% intermediate variable for display
Cv_disp = sym('Cv_disp',[3 3]).*nonzero_mask(Cv);

syms Kfst L1st
Ci_num = [TU1a,TU1b,TU1c]*Kfst*[eye(3);vec_atob;vec_atoc]+...
    [TI1a,TI1b,TI1c]*[TL1*eye(3);TL1*vec_atob;TL1*vec_atoc]+...
    [TI1a,TI1b,TI1c]*L1st*[eye(3);vec_atob;vec_atoc];
Ci = Ci_num/C_den;
Ci = simplify(subs(Ci,a,2*pi/3));
% intermediate variable for display
Ci_disp = sym('Ci_disp',[3 3]).*nonzero_mask(Ci);

syms Hv_s0 %TL(s-0*jw1)
syms Hv_s1 %TL(s-1jw1)
syms Hv_sn1 %TL(s+1jw1)
assume([Hv_sn1,Hv_s0,Hv_s1],'real')
HvB = [Hv_sn1,Hv_s0,Hv_s1];
THv = diag(HvB);

F = Km*(TM1a+TVdc0*Tcosf1t_a*THi*THv);
% intermediate variable for display
F_disp = sym('F_disp',[3 3]).*nonzero_mask(F);

%% close loop
Yden = (inv(G)+(H+F*Ci));
Ynum = (Kf*diag(ones(1,3))-S-F*Cv);

Yden_disp = (inv(G)+(H_disp+F_disp*Ci_disp));
Ynum_disp = (Kf*diag(ones(1,3))-S_disp-F_disp*Cv_disp);



% 
fp = fopen('Ydisp.txt','w');
fprintf(fp,'%s \n',latex(Yden_disp));
fprintf(fp,'%s \n',latex(Ynum_disp));
fprintf(fp,'%s \n',latex(F));
fprintf(fp,'%s \n',latex(Cv));
fprintf(fp,'%s \n',latex(Ci));
fprintf(fp,'%s \n',latex(S));
fprintf(fp,'%s \n',latex(H));
fclose(fp);
% save admit_load.mat Y TGi TGv Km TTpll TH G GiB GvB KmB TpllB HB L1B KfB D1d D1q I1d I1q Kdq Vdc Gv50

